import sensor
import image
import time
from pyb import UART
import math
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.HVGA)
sensor.set_windowing(120, 20, 280, 280)
sensor.skip_frames(time=1000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(False)
uart=UART(3,9600)
clock = time.clock()
black_threshold=()
white_threshold=()
red_white_threshold=(90, 100, -20, 20, -20, 20)
red_black_threshold=(24, 100, 88, 13, 80, -51)
green_white_threshold=(90, 100, -128, -20, -20, 20)
red_white_g_threshold=(81, 100, 1,47,0, 20)
red_black_g_threshold=(31, 62, 17, 42, -27, 24)
[x0,y0]=[0,0]
ROI=(0,0,280,280)
red=[]
first_check=1
threshold=100

def biankuang():
    thresholds = [(0, 100)]
    sensor.set_pixformat(sensor.GRAYSCALE)
    sensor.skip_frames(10)
    if (sensor.get_id() == sensor.OV7725):
        sensor.__write_reg(0xAC, 0xDF)
        sensor.__write_reg(0x8F, 0xFF)
    i=3
    rect=[]
    while i:
        img = sensor.snapshot().lens_corr(1.05)
        img.median(3, threshold=True, invert=True, percentile=0.4, offset=2)
        img.binary(thresholds)
        img.erode(2)
        img.dilate(2)
        rects = img.find_rects([0, 0, 280, 280], threshold=600000)
        if rects != []:
            img.draw_rectangle(rects[0].x(), rects[0].y(), rects[0].w(), rects[0].h(), color=[0, 255, 0])
            rect.append(rects[0].corners())
            i=i-1
            print(i)
    cxx=cyy=0
    cornerss=[0,0,0,0,0,0,0,0]
    for circ in range(3):
        for c in range(4):
            cxx=cxx+rect[circ][c][0]/12
            cyy=cyy+rect[circ][c][1]/12
    for cir in range(3):
        for ci in range(4):
            if rect[cir][ci][0]<cxx and rect[cir][ci][1]<cyy:
                cornerss[0]=cornerss[0]+rect[cir][ci][0]/3
                cornerss[1]=cornerss[1]+rect[cir][ci][1]/3
                cornerss[2]=cornerss[2]+rect[cir][ci-1][0]/3
                cornerss[3]=cornerss[3]+rect[cir][ci-1][1]/3
                cornerss[4]=cornerss[4]+rect[cir][ci-2][0]/3
                cornerss[5]=cornerss[5]+rect[cir][ci-2][1]/3
                cornerss[6]=cornerss[6]+rect[cir][ci-3][0]/3
                cornerss[7]=cornerss[7]+rect[cir][ci-3][1]/3
                break
    print (cornerss)
    return cornerss,[cxx,cyy]

def redcenter(img):
    global first_check
    global red
    global threshold
    blobs = img.find_blobs([red_white_threshold],pixels_threshold=5, area_threshold=5, merge=True)
    if blobs:
        max_blob = max(blobs, key=lambda b: b.pixels())
        x, y = max_blob.cx(), max_blob.cy()
        img.draw_cross(x,y,color=[0,255,255])
        if first_check==1:
            red=[x-x0, y-y0]
            first_check=0
            return [x-x0, y-y0]
        else:
            if x-x0-red[0]>threshold or y-y0-red[1]>threshold:
                return red
            else:
                red=[x-x0, y-y0]
                return [x-x0, y-y0]
    else:return None
def redcenter1(img):
    global first_check
    global red
    global threshold
    blobs = img.find_blobs([red_white_g_threshold],roi=ROI,pixels_threshold=3, area_threshold=3, merge=True)
    if blobs:
        max_blob = max(blobs, key=lambda b: b.pixels())
        x, y = max_blob.cx(), max_blob.cy()
        img.draw_cross(x,y,color=[0,255,255])

        if first_check==1:
            red=[x-x0, y-y0]
            first_check=0
            return [x-x0, y-y0]
        else:
            if x-x0-red[0]>threshold or y-y0-red[1]>threshold:
                return red
            else:
                red=[x-x0, y-y0]
                return [x-x0, y-y0]
    else:return None
def redblack(img):
    global first_check
    global red
    global threshold
    blobs = img.find_blobs([red_black_threshold], pixels_threshold=3, area_threshold=3, merge=True)
    if blobs:
        max_blob = max(blobs, key=lambda b: b.pixels())
        x, y = max_blob.cx(), max_blob.cy()
        img.draw_cross(x,y,color=[0,255,255])
        if first_check==1:
            red=[x-x0, y-y0]
            first_check=0
            return [x-x0, y-y0]
        else:
            if x-x0-red[0]>threshold or y-y0-red[1]>threshold:
                return red
            else:
                red=[x-x0, y-y0]
                return [x-x0, y-y0]
    else:return None
def redblack1(img):
    global first_check
    global red
    global threshold
    blobs = img.find_blobs([red_black_g_threshold],roi=ROI, pixels_threshold=5, area_threshold=5, merge=True)
    if blobs:
        max_blob = max(blobs, key=lambda b: b.pixels())
        x, y = max_blob.cx(), max_blob.cy()
        img.draw_cross(x,y,color=[0,255,255])
        if first_check==1:
            red=[x-x0, y-y0]
            first_check=0
            return [x-x0, y-y0]
        else:
            if x-x0-red[0]>threshold or y-y0-red[1]>threshold:
                return red
            else:
                red=[x-x0, y-y0]
                return [x-x0, y-y0]
    else:return None
def juxing(img):
    RECTANGLE_THRESHOLD = 100000
    point=[]
    points=[]
    while point==[]:
        img=sensor.snapshot().lens_corr(1.05)
        for r in img.find_rects(threshold=RECTANGLE_THRESHOLD):
            corners = r.corners()
            img.draw_rectangle(r.rect(), color=(255, 0, 0))
            for corner in corners:
                img.draw_circle(corner[0], corner[1], 5, color=(0, 255, 0))
            def sort_corners(corners):
                cx = sum(c[0] for c in corners) / 4
                cy = sum(c[1] for c in corners) / 4
                return sorted(corners, key=lambda c: math.atan2(c[1]-cy, c[0]-cx))
            sorted_corners = sort_corners(corners)
            top_left, top_right, bottom_right, bottom_left = sorted_corners
            def calculate_angle(p1, p2):
                dx = p2[0] - p1[0]
                dy = p2[1] - p1[1]
                angle = math.degrees(math.atan2(dy, dx)) % 180
                return angle
            def normalize_vector(v):
                length = math.sqrt(v[0]**2 + v[1]**2)
                return (v[0]/length, v[1]/length) if length > 0 else (0, 0)
            def get_inward_normal(p1, p2):
                dx, dy = p2[0]-p1[0], p2[1]-p1[1]
                return normalize_vector((-dy, dx))
            offset = 7
            inward_corners = []
            edges = [
                (top_left, top_right),
                (top_right, bottom_right),
                (bottom_right, bottom_left),
                (bottom_left, top_left)
            ]
            normals = [get_inward_normal(e[0], e[1]) for e in edges]
            for i in range(4):
                n1 = normals[i]
                n2 = normals[(i-1) % 4]
                dir_x = (n1[0] + n2[0]) / 2
                dir_y = (n1[1] + n2[1]) / 2
                dir_len = math.sqrt(dir_x**2 + dir_y**2)
                if dir_len > 0:
                    scale = offset / dir_len
                    inward_corners.append((
                        int(sorted_corners[i][0] + dir_x * scale),
                        int(sorted_corners[i][1] + dir_y * scale)
                    ))
            for i in range(4):
                point.append([inward_corners[i][0]-x0, inward_corners[i][1]-y0])
                points.append([inward_corners[i][0], inward_corners[i][1]])
            print(points)
            return point

def send1(c):
    ch=[int(c[0][0]),int(c[0][1]),int(c[1][0]),int(c[1][1])]
    chf = [f"{num:03d}" for num in ch]
    msgf=" ".join(chf)
    msg=f"@R{msgf}#"
    uart.write(msg)
    print(msg)
def send2(c):
    ch=[]
    for i in range(len(c)):
        ch.append(int(c[i][0]))
        ch.append(int(c[i][1]))
    chf = [f"{num:03d}" for num in ch]
    msgf=" ".join(chf)
    msg=f"@L{msgf}#"
    uart.write(msg)
    print(msg)
def send3(ch):
    c=[int(ch[0]),int(ch[1])]
    chf=[f"{num:03d}" for num in c]
    chf=" ".join(chf)
    msg=f"@N{chf}#"
    uart.write(msg)
    print(msg)
juxing_executed = False
timenum=1

while True:
    clock.tick()
    img = sensor.snapshot().lens_corr(1.05)
    while timenum==1:
        cns,[centerx,centery]=biankuang()
        sensor.set_pixformat(sensor.RGB565)
        sensor.skip_frames(time=100)
        timenum=0
        [x0,y0]=[cns[0],cns[1]]
    if uart.any():
        print("OOOOOOOOOOOOO")
        info=int(uart.read())
        print (info)
        if info==1:
            redp=None
            while (redp==None):
                img=sensor.snapshot().lens_corr(1.05)
                redp=redcenter(img)
                if not redp:
                    redp=redblack(img)
            send1([[centerx-x0,centery-y0],redp])
            while (uart.any()==0):
                img=sensor.snapshot().lens_corr(1.05)
                uyt=redcenter(img)
                if uyt:
                    send3(uyt)
        elif info==2:
            redp2=None
            while (redp2==None):
                img=sensor.snapshot().lens_corr(1.05)
                redp2=redcenter(img)
                if not redp2:
                    redp2=redblack(img)
            send2([[0,0],[cns[2]-x0,cns[3]-y0],[cns[4]-x0,cns[5]-y0],[cns[6]-x0,cns[7]-y0]])
            print([[0,0],[cns[2]-x0,cns[3]-y0],[cns[4]-x0,cns[5]-y0],[cns[6]-x0,cns[7]-y0]])
            while(uart.any()==0):
                img=sensor.snapshot().lens_corr(1.05)
                uyt=redcenter(img)
                if uyt:
                    send3(uyt)
        elif info == 3 or info == 4:
            if juxing_executed==False:
                rect_points = juxing(img)
                if rect_points:
                    send2(rect_points)
                    juxing_executed = True
            while (uart.any()==0):
                img=sensor.snapshot().lens_corr(1.05)
                wyx=redcenter(img)
                if wyx:
                    send3(wyx)
                else:
                    wyx=redblack(img)
                    if wyx:
                        send3(wyx)
            juxing_executed = False
        elif info == 6 or info ==5:
            if juxing_executed==False:
                rect_points = juxing(img)
                if rect_points:
                    send2(rect_points)
                    juxing_executed = True
            while (uart.any()==0):
                img=sensor.snapshot().lens_corr(1.05)
                wyx=redcenter1(img)
                if wyx:
                    send3(wyx)
                else:
                    wyx=redblack1(img)
                    if wyx:
                        send3(wyx)
            juxing_executed = False
